//#define _CRT_SECURE_NO_WARNINGS
#include <glew.h>
#include <GLFW/glfw3.h>
#include <iostream>
#include <math.h>
#include <algorithm>
#include <stack>
#include "config.h"
#include "model.h"
#include "util.h"
#include<random>
#include<vector>
#include<memory>
#include<cmath>
#include"AStar.h"
#include <fstream>
#include <QRect>
#include <Qpoint>
#include <map>
#include <string>
#include <sstream>
//#include <filesystem>
//#define N_indiv 300//人数
#define N_grid 60//一条边上分割的格子总数
#define DIVIDER 2//格子的分辨率
#define SEARCH_SCOPE 2//计算个体的受力时考虑的范围
#define MAX_INDIVI_PER_GRID 10//每个格子中个体的数量
#define K_fij 1//个体间的作用力的系数K
#define k_fij 1//个体间的作用力的系数k
#define K_group 0.01//group力的系数K
//using namespace std;
bool start_simulation = false;
int lasttime = 0;
int hurtcount = 0;
int starttime=0;
class Individual {//个体agnet类，管理每个个体的位置

public:
	float r_body;//模拟人的大小
	float A, B;
	float AL, DE;//需要帮助和帮助他人指数
	int id;//个体的id
	float max_vel;
	bool has_shadow = true;
	int help_id=-1;//帮助了谁
	int helped_id=-1;//被谁帮助
private:
	
	glm::vec3 translate{ 0,0,0 };//充当位置
	int texture = -1;//texture的id,同时充当family的作用
	glm::vec3 vel = { 0,0,0 };//速度
	glm::vec3 direction = {0,0,1.0};//初始的朝向
public:
	Individual() {};
	Individual(int id,int texture, glm::vec3 vel) :id(id),texture(texture),vel(vel)
	{};
	static float getAngle(glm::vec3& prevel, glm::vec3& vel);//获取从pre方向到vel方向旋转的角度（角度制）
	glm::vec3& getVel() { return vel; }//获取速度
	glm::vec3& getPreDir() { return direction; }//返回上一次的方向
	void bindTexture(int textureid) {
		texture = textureid;
	}
	int getTexture() { return texture; }
	void setTran(const glm::vec3 &tran) {
		translate = tran;
	}
	glm::vec3& getTran()
	{ return translate; } 
};
class Map//地图类，管理格子
{
private:
	int* grid_count = new int[N_grid * N_grid]{0};//每个格子中的个体数量
	int* grid_indiv = new int[N_grid * N_grid * MAX_INDIVI_PER_GRID] ;//每个格子中的个体编号


public:
	Map(std::vector<QRect*>& wall_list, std::vector<QRect*>& extra_wall_list)
	{
		initObstacle(wall_list);
		directions.reserve(N_grid * N_grid);
		int* dire;
			AStar s(N_grid, has_obstacle);
			s.WrToFile("log.txt");
			dire = s.get_path();
		
		for (int i = 0; i < N_grid * N_grid; i++)
		{
			if (dire[i] == 0)directions.push_back(glm::normalize(glm::vec3(0,0,-1)));
			else if (dire[i] == -1)directions.push_back(glm::normalize(glm::vec3(0, 0, 1)));
			else if (dire[i] == 1)directions.push_back(glm::normalize(glm::vec3(0, 0, 1)));
			else if (dire[i] == 2)directions.push_back(glm::normalize(glm::vec3(-1, 0, 0)));
			else if (dire[i] == 3)directions.push_back(glm::normalize(glm::vec3(1, 0, 0)));
			else if (dire[i] == 4)directions.push_back(glm::normalize(glm::vec3(-1, 0, -1)));
			else if (dire[i] == 5)directions.push_back(glm::normalize(glm::vec3(1, 0, -1)));
			else if (dire[i] == 6)directions.push_back(glm::normalize(glm::vec3(-1, 0, 1)));
			else if (dire[i] == 7)directions.push_back(glm::normalize(glm::vec3(1, 0, 1)));
		}
		
		add_extra_obstacle(extra_wall_list);

	}
	static int getID(float x,float z);//获取grid的编号,输入的是中心点的坐标
	int getID(int c_x, int c_z);//获取grid的编号,输入的是编号
	static void getcoordinate(int id,float &x,float& z);//输入grid的编号，输出grid的坐标
	//float coordinate[N_grid * N_grid];//格子编号从左下角开始
	//void initCoordinate();//初始化格子的中心坐标（一直保持不变）
	//障碍物相关
	bool has_obstacle[N_grid * N_grid];//格子中有无障碍物
	void initObstacle(std::vector<QRect*>& wall_list);//初始化障碍物的位置
	void setObstacle(int id);//在给定编号的位置设置一个障碍物
	void removeObstacle(int id);//移除给定位置的障碍物
	void remove_extra_obstacle();//关闭额外的墙
	void add_extra_obstacle(std::vector<QRect*>& extra_wall_list);//放置额外的墙
	bool is_Obstacle(float x,float z);//当前输入的坐标里面是否有障碍物。
	std::vector<int> getNearByObstacle(Individual indivi);//返回周围的障碍物的位置
	std::vector<int> obstacle;//保存了初始障碍物的所在的编号，为了加快渲染速度
	std::vector<int> extra_obstacle;//保存了额外的障碍物所在的位置
	//个体相关
	void flashIndivPalce();//更新每个个体的位置
	void putIndiv2Grid();//将个体的编号放入对应的格子的数据结构中
	std::vector<int> getNearByIndiv(Individual indivi);//返回周围的个体
	//寻路
	std::vector<glm::vec3> directions;


};
//地图相关
std::shared_ptr<Map> mymap;
//个体相关
int indvi_texture[3];//个体的纹理
Individual * individuals;//个体类型的数组

//全局配置相关
Config config;						//创建配置文件
glm::mat4 model, view, projection;	//创建变换矩阵
stack<glm::mat4> mat_stack;
//天空盒相关
Model* bgModel;		//背景（天空盒模型）
Shader* bgShader;	//背景着色器
Shader* depthShader;//
//阴影生成相关
unsigned int depthMap;//深度图texture，作为FBO的深度缓冲区
const unsigned int SHADOW_WIDTH = 1024, SHADOW_HEIGHT = 1024;
unsigned int depthMapFBO;
glm::mat4 lightSpaceMatrix;//转化到光线视角下的view和投影矩阵的乘积

float offsetx, offsetz;//个体的平移位置，没用也没关系
//绘制天空盒
void draw_background(glm::mat4, glm::mat4);
//绘制模型的线框
void draw_line(Model*, glm::mat4&, glm::mat4&, glm::mat4&);
//回调函数
void cursor_callback(GLFWwindow*, double, double);
void key_callback(GLFWwindow*, int, int, int, int);
void scroll_callbcak(GLFWwindow*, double, double);
//受力相关
glm::vec3 calcFOther(Individual& individual);
glm::vec3 calcFObstacle(Individual& individual);
glm::vec3 calcGroupForce(Individual& individual);
bool is_in_obstacle(glm::vec3 pos);
int updateIndividuals(double currentFrameTime, std::string& outstr);


//个人实验
unsigned int spaceship_texture;
Shader* spaceship_shader;
Model* spaceship_model;

void init(std::map<QPoint*, int>& indiv_list, std::vector<QRect*>& wall_list, std::vector<QRect*>& extra_wall_list) {


	//1.创建着色器
	config.lineShader = new Shader("shaders/line_v.shader", "shaders/line_f.shader");	//线框着色器
	config.shaders.push_back(new Shader("shaders/ori_v.shader", "shaders/ori_f.shader"));//默认着色器（无纹理）
	config.shaders.push_back(new Shader("shaders/lumi_v.shader", "shaders/lumi_f.shader"));//自发光物体着色器
	config.shaders.push_back(new Shader("shaders/point_v.shader", "shaders/point_f.shader"));//点光源着色器
	//2.读取models文件夹下的模型、纹理文件
	vector<string> model_file_name = Util::getFiles("models", ".obj", true);//获取文件夹下面的模型文件的名字
	//3.加载每一个模型，放到config的模型库里面
	for (auto filename : model_file_name) {
		//3.1加载模型（无texture）
		string path = "models\\";
		path.append(filename);
		config.models.push_back(new Model((path +".obj").c_str(), config.shaders[0]));
		//3.2加载texture，这个texture的名字是和对应的模型的名字是一样的
		string tex_path = Util::find_postfix(path);
		//3。3如果没有对应的纹理就用无纹理的program
		if (!tex_path.empty()) 
			config.models.back()->shader = config.shaders[2];
		config.models.back()->bindTexture((path + tex_path).c_str());
		
	}

	//9.初始化场景对象，障碍物的位置是在构造函数中预先加载的
	mymap = std::make_shared<Map>(wall_list, extra_wall_list);
	
	//4.配置opengl
	//glEnable(GL_CULL_FACE);				//清除面的背面显示,被这玩意坑死了
	glEnable(GL_POLYGON_OFFSET_LINE);	//OpenGL会在绘制多边形时自动将多边形向视点方向进行偏移，以避免深度缓冲区的精度问题导致的多边形重叠问，就是减少z fight用的
	glPolygonOffset(-2.0f, -2.0f);
	glEnable(GL_DEPTH_TEST);			//开启深度测试
	glClearColor(0.2f, 0.3f, 0.3f, 1.0f);	

	//4.加载不同个体的texture
	indvi_texture[0] = Util::loadTexture("models/orange.png");
	indvi_texture[1] = Util::loadTexture("models/lemen.png");
	indvi_texture[2] = Util::loadTexture("models/cow.png");
	//5.初始化各个individual的类，本来想用instance的，不过传入很多个uniform不太行
	int num = 0;
	auto iter_indiv_list = indiv_list.begin();
	for (int num_i=0; num_i < config.N_indiv; num_i++)
	{
		
		auto& i = individuals[num_i];
		//i.setTran({rand()%100-60,1,rand() % 100 - 60 });//绕z轴旋转后需要x+1补偿*****，别忘了，不然会导致计算力的时候出问题
		i.setTran({ iter_indiv_list->first->x()/5-60,1,iter_indiv_list->first->y()/5-60 });
		while(is_in_obstacle(i.getTran()))i.setTran({ rand() % 100 - 60,1,rand() % 100 - 60 });
		i.bindTexture(indvi_texture[iter_indiv_list->second]);
		i.r_body = 1;
		i.A = 1;
		i.B = 1;
		i.id = num;
		i.max_vel = 0.1;
		i.DE = ((1 + rand() % 80) / 101.0);
		//i.DE = 0.1;//速度为最大值会报错,因为浮点数是不精确的
		i.AL = ((1 + rand() % 100) / 101.0);
		num++;
		iter_indiv_list++;
	}
	//6.初始化相机的位置，之前的位置太不好了
	config.cameraPos = { 0, 50, 70.f };
	config.pitch= -89.9;
	//7.创建天空盒
	bgShader = new Shader("shaders/skybox_v.shader", "shaders/skybox_f.shader");
	bgModel = new Model("models/box1.obj", bgShader);

	bgModel->bindTexture("models/orange.png");
	//8.设置光源shader
	for (auto each : config.models) {//
		//render grass

		if (each->name == "box2.obj")//make the scene bigger
		{
			//each->scale = { 10,10,10 };
			each->bindShader(bgShader);
				

		}
		//if(each->name=="box1.obj")each->shader = config.shaders[0];
	}
	
	
	//10.初始化光源位置，这个位置要选好，需要看到所有的场景，不然阴影生成算法会出问题（我想不到改进方法）
	config.lightPos = { 0,140,60 };

	//11初始化阴影生成相关的代码
	//11.1设置FBO和创建texture

	glGenFramebuffers(1, &depthMapFBO);
	// create depth texture

	glGenTextures(1, &depthMap);
	glBindTexture(GL_TEXTURE_2D, depthMap);
	glTexImage2D(GL_TEXTURE_2D, 0, GL_DEPTH_COMPONENT, SHADOW_WIDTH, SHADOW_HEIGHT, 0, GL_DEPTH_COMPONENT, GL_FLOAT, NULL);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
	// attach depth texture as FBO's depth buffer
	glBindFramebuffer(GL_FRAMEBUFFER, depthMapFBO);
	glFramebufferTexture2D(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_TEXTURE_2D, depthMap,0);
	glDrawBuffer(GL_NONE);
	glReadBuffer(GL_NONE);
	glBindFramebuffer(GL_FRAMEBUFFER, 0);
	//11.2设置shader 
	depthShader = new Shader("shaders/depth_v.shader", "shaders/depth_f.shader");

	//个人实验,暂时不用做什么

	
}


void display(double currentFrameTime) {
	//1.更新时间和相机位置和方向
	static double lastFrameTime;
	config.updataCamera(currentFrameTime - lastFrameTime);//防止电脑不同导致相机移动的速率不同
	lastFrameTime = currentFrameTime; 

	//2.更新view变换矩阵
	view = glm::lookAt(config.cameraPos, config.cameraPos + config.cameraDir, config.cameraUp);
	//view = glm::lookAt(config.lightPos, glm::vec3(0,0,10), config.cameraUp);
	//创建projection变换矩阵
	projection = glm::perspective(glm::radians(config.fovY), (float)config.windowWidth / config.windowHeight, config.zNear, config.zFar);
	//3.更新物体平移变换（来自model 变换）
	draw_background(view,projection);

	for (auto each : config.models) {//绘制所有模型
		//render grass

		if (each->name == "grass.obj")//make the scene bigger
		{
			//each->scale = { 10,10,10 };
			model = glm::mat4(1.0f);
			model = glm::scale(model, each->scale);
			//为每个模型对应的着色器程序传入model、view、projection变换矩阵

			each->shaderSetMatrixes(model, view, projection);

			//传入光源和摄像机信息
			
			each->shader->setVec3("PointLightPos", config.lightPos);
			each->shader->setVec3("cameraPos", config.cameraPos);
			each->shader->setMat4("lightSpaceMatrix", lightSpaceMatrix);

			//设置texture编号
			//glUniform1i(glGetUniformLocation(each->shader->id, ""), 0);

			if (each->texture > 0) {
				glActiveTexture(GL_TEXTURE0);
				glBindTexture(GL_TEXTURE_2D, each->texture);
			}
			glActiveTexture(GL_TEXTURE1);
			glBindTexture(GL_TEXTURE_2D, depthMap);
			glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
			for (auto mesh : each->meshes) {
				mesh.draw(each->shader->id);
			}
			glBindTexture(GL_TEXTURE_2D, 0);
			//draw_line(each, model, view, projection);			//绘制模型线框

		}
		//个人实验
		//if (each->name == "spaceship.obj")//make the scene bigger
		//{
		//	each->scale = { 10,10,10 };
		//	model = glm::mat4(1.0f);

		//	model = glm::translate(model, config.space_ship_translate);
		//	model = glm::rotate(model, config.space_ship_rotate, glm::vec3(0.0, 0.0, 1.0));
		//	model = glm::scale(model, each->scale);
		//	//为每个模型对应的着色器程序传入model、view、projection变换矩阵


		//	each->shaderSetMatrixes(model, view, projection);

		//	//传入光源和摄像机信息

		//	each->shader->setVec3("PointLightPos", config.lightPos);
		//	each->shader->setVec3("cameraPos", config.cameraPos);
		//	each->shader->setMat4("lightSpaceMatrix", lightSpaceMatrix);

		//	//设置texture编号
		//	//glUniform1i(glGetUniformLocation(each->shader->id, ""), 0);

		//	if (each->texture > 0) {
		//		glActiveTexture(GL_TEXTURE0);
		//		glBindTexture(GL_TEXTURE_2D, each->texture);
		//	}
		//	glActiveTexture(GL_TEXTURE1);
		//	glBindTexture(GL_TEXTURE_2D, depthMap);
		//	glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
		//	for (auto mesh : each->meshes) {
		//		mesh.draw(each->shader->id);
		//	}
		//	glBindTexture(GL_TEXTURE_2D, 0);
		//	//draw_line(each, model, view, projection);			//绘制模型线框

		//}
		else if (each->name == "box1.obj")
		{
			//render each individual
			for (int i=0; i < config.N_indiv; i++)
			{
				if (!individuals[i].has_shadow)continue;


				each->rotate_y.w = currentFrameTime * 100;
				//model变换矩阵
				model = glm::mat4(1.0f);
				model = glm::scale(model, each->scale);
				
			/*	individuals[i].getTran().x = individuals[i].getTran().x > 58 ? 58 : individuals[i].getTran().x;
				individuals[i].getTran().z = individuals[i].getTran().z > 58 ? 58 : individuals[i].getTran().z;
				individuals[i].getTran().x = individuals[i].getTran().x < -58 ? -58 : individuals[i].getTran().x;
				individuals[i].getTran().z = individuals[i].getTran().z < -58 ? -58 : individuals[i].getTran().z;*/
				
				model = glm::translate(model, individuals[i].getTran());

				model = glm::translate(model, glm::vec3(1, 0, 0));//补偿旋转带来的位置偏移
				//rotate to the vel direction,need to be optimazied!!!!!!!
				//test 
				/*auto t1 = glm::vec3(0, 0, 1);
				auto t2 = glm::vec3(0.7, 0, -0.7);
				float tmp=Individual::getAngle(t1, t2);*/
				//思来想去还是把旋转注释掉吧，加了后效果反而很奇怪
				//if (glm::abs(individuals[i].getVel().x) > 0.01 || glm::abs(individuals[i].getVel().z) > 0.01)
				//model = glm::rotate(model, Individual::getAngle(individuals[i].getPreDir(), individuals[i].getVel()), glm::vec3(0.0, 1.0, 0.0));//速度为0的时候不会显示是由这个导致的
				//model = glm::rotate(model, Individual::getAngle(t1, individuals[i].getVel()), glm::vec3(0.0, 1.0, 0.0));
				model = glm::rotate(model, glm::radians(90.0f), glm::vec3(0.0, 0.0, 1.0));
				//为每个模型对应的着色器程序传入model、view、projection变换矩阵

				each->shaderSetMatrixes(model, view, projection);

				//传入光源和摄像机信息
				each->shader->setVec3("PointLightPos", config.lightPos);
				each->shader->setVec3("cameraPos", config.cameraPos);
				each->shader->setMat4("lightSpaceMatrix", lightSpaceMatrix);
				//设置texture
				each->texture = individuals[i].getTexture();

				if (each->texture > 0) {
					glActiveTexture(GL_TEXTURE0);
					glBindTexture(GL_TEXTURE_2D, each->texture);
				}
				glActiveTexture(GL_TEXTURE1);
				glBindTexture(GL_TEXTURE_2D, depthMap);
				glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
				for (auto mesh : each->meshes) {
					mesh.draw(each->shader->id);
				}
				glBindTexture(GL_TEXTURE_2D, 0);
				//draw_line(each, model, view, projection);			//绘制模型线框
			}
		}
		else if (each->name == "box2.obj")
		{
			model = glm::mat4(1.0f);
			glm::vec3 temp_translate = config.lightPos;
			model = glm::translate(model, temp_translate);
			//为每个模型对应的着色器程序传入model、view、projection变换矩阵

			each->shaderSetMatrixes(model, view, projection);

			//传入光源和摄像机信息
			each->shader->setVec3("PointLightPos", config.lightPos);
			each->shader->setVec3("cameraPos", config.cameraPos);


			each->draw();
			//draw_line(each, model, view, projection);			//绘制模型线框
		}
		else if (each->name == "wall.obj")
		{
			for (int i = 0; i < mymap->obstacle.size(); i++)
			{
				


				float x, z;
				Map::getcoordinate(mymap->obstacle[i],x,z);
				each->rotate_y.w = currentFrameTime * 100;
				//model变换矩阵
				model = glm::mat4(1.0f);
				model = glm::scale(model, each->scale);
				glm::vec3 temp_translate = {x,0,z};

				model = glm::translate(model, temp_translate);
				//为每个模型对应的着色器程序传入model、view、projection变换矩阵

				each->shaderSetMatrixes(model, view, projection);

				//传入光源和摄像机信息
				each->shader->setVec3("PointLightPos", config.lightPos);
				each->shader->setVec3("cameraPos", config.cameraPos);
				each->shader->setMat4("lightSpaceMatrix", lightSpaceMatrix);

				if (each->texture > 0) {
					glActiveTexture(GL_TEXTURE0);
					glBindTexture(GL_TEXTURE_2D, each->texture);
				}
				glActiveTexture(GL_TEXTURE1);
				glBindTexture(GL_TEXTURE_2D, depthMap);
				glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
				for (auto mesh : each->meshes) {
					mesh.draw(each->shader->id);
				}
				glBindTexture(GL_TEXTURE_2D, 0);
				//draw_line(each, model, view, projection);			//绘制模型线框
			}

			for (int i = 0; i < mymap->extra_obstacle.size(); i++)
			{
				float x, z;
				Map::getcoordinate(mymap->extra_obstacle[i], x, z);
				each->rotate_y.w = currentFrameTime * 100;
				//model变换矩阵
				model = glm::mat4(1.0f);
				model = glm::scale(model, each->scale);
				glm::vec3 temp_translate = { x,0,z };

				model = glm::translate(model, temp_translate);
				//为每个模型对应的着色器程序传入model、view、projection变换矩阵

				each->shaderSetMatrixes(model, view, projection);

				//传入光源和摄像机信息
				each->shader->setVec3("PointLightPos", config.lightPos);
				each->shader->setVec3("cameraPos", config.cameraPos);
				each->shader->setMat4("lightSpaceMatrix", lightSpaceMatrix);

				if (each->texture > 0) {
					glActiveTexture(GL_TEXTURE0);
					glBindTexture(GL_TEXTURE_2D, each->texture);
				}
				glActiveTexture(GL_TEXTURE1);
				glBindTexture(GL_TEXTURE_2D, depthMap);
				glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
				for (auto mesh : each->meshes) {
					mesh.draw(each->shader->id);
				}
				glBindTexture(GL_TEXTURE_2D, 0);
				//draw_line(each, model, view, projection);			//绘制模型线框
			}

		}
		
		
	}
}
int getGridId(glm::vec3& pos)
{
	return glm::floor((pos.x) / DIVIDER) + (glm::floor((pos.z) / DIVIDER) + N_grid / 2) * N_grid + N_grid / 2;
}
bool is_destinnation(int grid_id)
{
	
	return (grid_id == N_grid * N_grid - N_grid / 2 || grid_id == N_grid * N_grid - N_grid / 2 + 1 || grid_id == N_grid * N_grid - N_grid / 2 - 1);
}
bool is_in_destination(glm::vec3& pos)
{
	auto pos1 = pos + glm::vec3(1, 0, 1);//get the front pos
	auto pos2 = pos + glm::vec3(-1, 0, 1);//get the front pos
	auto pos3 = pos + glm::vec3(1, 0, -1);//get the front pos
	auto pos4 = pos + glm::vec3(-1, 0, -1);//get the front pos
	if (is_destinnation(getGridId(pos1)) || is_destinnation(getGridId(pos2)) || is_destinnation(getGridId(pos3)) || is_destinnation(getGridId(pos4)))
		return true;
	else
		return false;
	
}

bool is_in_obstacle(glm::vec3 pos)
{
	auto pos1 = pos + glm::vec3(1, 0, 1);//get the front pos
	auto pos2 = pos + glm::vec3(-1, 0, 1);//get the front pos
	auto pos3 = pos + glm::vec3(1, 0, -1);//get the front pos
	auto pos4 = pos + glm::vec3(-1, 0, -1);//get the front pos
	if (mymap.get()->has_obstacle[getGridId(pos1)] || mymap.get()->has_obstacle[getGridId(pos2)] || mymap.get()->has_obstacle[getGridId(pos3)] || mymap.get()->has_obstacle[getGridId(pos4)])
		return true;
	else
		return false;
	
}
void checkObstacle(Individual& indiv)//TODO
{
	//获取当前个体所在的格子
	//auto norm = glm::normalize(indiv.getVel());
	if (is_in_obstacle(indiv.getTran()))
	{
		auto pos1 = indiv.getTran() + glm::vec3(1, 0, 1);//get the front pos
		auto pos2 = indiv.getTran() + glm::vec3(-1, 0, 1);//get the front pos
		auto pos3 = indiv.getTran() + glm::vec3(1, 0, -1);//get the front pos
		auto pos4 = indiv.getTran() + glm::vec3(-1, 0, -1);//get the front pos
		if (mymap.get()->has_obstacle[getGridId(pos1)] && mymap.get()->has_obstacle[getGridId(pos3)] || mymap.get()->has_obstacle[getGridId(pos2)] && mymap.get()->has_obstacle[getGridId(pos4)])
		{
			indiv.getTran().x -= indiv.getVel().x;
		}
		if (mymap.get()->has_obstacle[getGridId(pos1)] && mymap.get()->has_obstacle[getGridId(pos2)] || mymap.get()->has_obstacle[getGridId(pos3)] && mymap.get()->has_obstacle[getGridId(pos4)])
		{
			indiv.getTran().z -= indiv.getVel().z;
		}
		if(indiv.getVel().x>0&&(mymap.get()->has_obstacle[getGridId(pos1)] || mymap.get()->has_obstacle[getGridId(pos3)]))
			indiv.getTran().x -= indiv.getVel().x;
		else if(indiv.getVel().x < 0 && (mymap.get()->has_obstacle[getGridId(pos2)] || mymap.get()->has_obstacle[getGridId(pos4)]))
			indiv.getTran().x -= indiv.getVel().x;
		if (indiv.getVel().z > 0 && (mymap.get()->has_obstacle[getGridId(pos1)] || mymap.get()->has_obstacle[getGridId(pos2)]))
			indiv.getTran().z -= indiv.getVel().z;
		else if (indiv.getVel().z < 0 && (mymap.get()->has_obstacle[getGridId(pos3)] || mymap.get()->has_obstacle[getGridId(pos4)]))
			indiv.getTran().z -= indiv.getVel().z;

	}
		
	
	
	
	
	
}
void getdepthmap() 
{


	//2.更新view变换矩阵
	view = glm::lookAt(config.lightPos, glm::vec3(0,0,10), config.cameraUp);
	//view = glm::lookAt(config.cameraPos, config.cameraPos + config.cameraDir, config.cameraUp);
	//创建projection变换矩阵
	projection = glm::perspective(glm::radians(config.fovY), (float)config.windowWidth / config.windowHeight, config.zNear, config.zFar);
	lightSpaceMatrix = projection * view;
	// render scene from light's point of view
	/*depthShader->use();
	depthShader->setMat4("lightSpaceMatrix", lightSpaceMatrix);*/

	glViewport(0, 0, SHADOW_WIDTH, SHADOW_HEIGHT);
	glBindFramebuffer(GL_FRAMEBUFFER, depthMapFBO);
	glClear(GL_DEPTH_BUFFER_BIT);
	glActiveTexture(GL_TEXTURE0);
	//glBindTexture(GL_TEXTURE_2D,indvi_texture[0]);
	//渲染
	for (auto each : config.models) {//绘制所有模型
		//render grass

		if (each->name == "grass.obj")//make the scene bigger
		{
			//each->scale = { 10,10,10 };
			model = glm::mat4(1.0f);
			model = glm::scale(model, each->scale);
			//为每个模型对应的着色器程序传入model、view、projection变换矩阵

			//传入uniform信息，另一个已经传过了
			depthShader->setMat4("model",model);
			depthShader->setMat4("lightSpaceMatrix", lightSpaceMatrix);
			//画
			glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
			for (auto mesh : each->meshes) {
				mesh.draw(depthShader->id);
			}

		}
		//个人实验*********************88
		//else if (each->name == "spaceship.obj")//make the scene bigger
		//{
		//	each->scale = { 10,10,10 };
		//	model = glm::mat4(1.0f);
		//	model = glm::translate(model, config.space_ship_translate);//space_ship_rotate
		//	model = glm::rotate(model, config.space_ship_rotate, glm::vec3(0.0, 0.0, 1.0));
		//	model = glm::scale(model, each->scale);
		//	//为每个模型对应的着色器程序传入model、view、projection变换矩阵

		//	//传入uniform信息，另一个已经传过了
		//	depthShader->setMat4("model", model);
		//	depthShader->setMat4("lightSpaceMatrix", lightSpaceMatrix);
		//	//画
		//	glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
		//	for (auto mesh : each->meshes) {
		//		mesh.draw(depthShader->id);
		//	}

		//}
		else if (each->name == "box1.obj")
		{
			//render each individual
			for (int i = 0; i < config.N_indiv; i++)
			{
				if (!individuals[i].has_shadow)continue;
				//注释掉的这部分放在专门的函数中处理
				/*offsetx = (rand() % 20 - 10) / 50.0;
				offsetz = (rand() % 20 - 10) / 50.0;
				individuals[i].getVel() = { offsetx ,0,offsetz };*/


				//model变换矩阵
				model = glm::mat4(1.0f);
				model = glm::scale(model, each->scale);
				

				individuals[i].getTran() += individuals[i].getVel();
				checkObstacle(individuals[i]);//通过速度和位置根据障碍物进行截断。如果个体进入到障碍物中就让他回退到合适的位置。
			
				model = glm::translate(model, individuals[i].getTran());
				model = glm::translate(model, glm::vec3(1, 0, 0));//补偿旋转带来的位置偏移
				//rotate to the vel direction

				//if (glm::abs(individuals[i].getVel().x) > 0.01 || glm::abs(individuals[i].getVel().z) > 0.01)
				//model = glm::rotate(model, Individual::getAngle(individuals[i].getPreDir(),individuals[i].getVel()), glm::vec3(0.0, 1.0, 0.0));

				model = glm::rotate(model, glm::radians(90.0f), glm::vec3(0.0, 0.0, 1.0));
				//为每个模型对应的着色器程序传入model、view、projection变换矩阵

				//传入uniform信息，另一个已经传过了
				depthShader->setMat4("model", model);
				depthShader->setMat4("lightSpaceMatrix", lightSpaceMatrix);
				//画
				glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
				for (auto mesh : each->meshes) {
					mesh.draw(depthShader->id);
				}
			}
		}
		else if(each->name == "wall.obj")
		{
			for (int i = 0; i < mymap->obstacle.size(); i++)
			{
				float x, z;
				Map::getcoordinate(mymap->obstacle[i], x, z);
				//model变换矩阵
				model = glm::mat4(1.0f);
				model = glm::scale(model, each->scale);
				glm::vec3 temp_translate = { x,0,z };

				model = glm::translate(model, temp_translate);
				//为每个模型对应的着色器程序传入model、view、projection变换矩阵

				//传入uniform信息，另一个已经传过了
				depthShader->setMat4("model", model);
				depthShader->setMat4("lightSpaceMatrix", lightSpaceMatrix);
				//画
				glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
				for (auto mesh : each->meshes) {
					mesh.draw(depthShader->id);
				}
			}

			for (int i = 0; i < mymap->extra_obstacle.size(); i++)
			{
				float x, z;
				Map::getcoordinate(mymap->extra_obstacle[i], x, z);
				//model变换矩阵
				model = glm::mat4(1.0f);
				model = glm::scale(model, each->scale);
				glm::vec3 temp_translate = { x,0,z };

				model = glm::translate(model, temp_translate);
				//为每个模型对应的着色器程序传入model、view、projection变换矩阵

				//传入uniform信息，另一个已经传过了
				depthShader->setMat4("model", model);
				depthShader->setMat4("lightSpaceMatrix", lightSpaceMatrix);
				//画
				glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
				for (auto mesh : each->meshes) {
					mesh.draw(depthShader->id);
				}
			}

		}


	}




	glBindFramebuffer(GL_FRAMEBUFFER, 0);

	// reset viewport
	glViewport(0, 0, (float)config.windowWidth, config.windowHeight);
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
}

//绑定回调函数
void set_callback(GLFWwindow* window) {
	glfwSetKeyCallback(window, key_callback);
	glfwSetCursorPosCallback(window, cursor_callback);
	glfwSetScrollCallback(window, scroll_callbcak);
}

int start(std::map<QPoint*, int>& indiv_list, std::vector<QRect*>& wall_list, std::vector<QRect*>& extra_wall_list, std::string& outstr) {
	//初始化glfw(窗口管理)
	if (!glfwInit()) {
		cerr << "error in glfwInit" << endl;
		return -1;
	}
	//指定版本
	glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 4);
	glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 3);

	//glfwWindowHint(GLFW_REFRESH_RATE, 360);
	//创建窗口与OpenGL上下文
	GLFWwindow* window = glfwCreateWindow(config.windowWidth, config.windowHeight, config.windowTitle, nullptr, nullptr);
	if (!window) {
		glfwTerminate();
		cerr << "error in glfwCreateWindow" << endl;
		return -1;
	}
	glfwMakeContextCurrent(window);
	glfwSwapInterval(1);		//在交换缓冲区之前从glfwSwapBuffers调用时选择驱动程序等待的最小监视器刷新次数。
									//如果间隔为零,则在调用glfwSwapBuffers时立即进行交换,而不等待刷新。否则,至少间隔回溯将在每个缓冲区交换之间传递。
									//当不希望测量等待垂直回扫所花费的时间时,使用零交换间隔对于基准测试目的是有用的。但是,交换间隔为1可以避免撕裂。
	set_callback(window);		//设置回调函数
	glfwSetInputMode(window, GLFW_CURSOR, GLFW_CURSOR_DISABLED);


	//初始化glew(函数加载)
	if (glewInit() != GLEW_OK) {
		cerr << "error in glewInit" << endl;
		return -2;
	}


	//接入图形窗口
	config.N_indiv = indiv_list.size();
	individuals = new Individual[config.N_indiv];

	init(indiv_list, wall_list, extra_wall_list);//初始化一些东西，我把那些只需要进行一次的操作都放到这里面了

	//渲染循环
	while (!glfwWindowShouldClose(window)) {
		glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);//清空深度缓冲区和颜色缓冲区
			mymap->flashIndivPalce();
			if(updateIndividuals(glfwGetTime(),outstr))glfwSetWindowShouldClose(window, GL_TRUE); ;//更新每个个体的状态
			getdepthmap();//计算光源视角下的深度图
			display(glfwGetTime());//渲染最后的图形
	

		glfwSwapBuffers(window);
		glfwPollEvents();
	}
	glfwDestroyWindow(window);
	glfwTerminate();
	return 0;
}

void key_callback(GLFWwindow* window, int key, int scancode, int action, int mods) {
	if (action == GLFW_PRESS) {
		config.keyboard[key] = true;
		if (key==GLFW_KEY_T) {
			mymap->remove_extra_obstacle();
			start_simulation = true;
			starttime = glfwGetTime();
		}
		
	}
	else if (action == GLFW_RELEASE) {
		config.keyboard[key] = false;
	}
}

void cursor_callback(GLFWwindow* window, double xpos, double ypos) {
	static double preX = xpos;
	static double preY = ypos;
	config.yaw += (xpos - preX) * config.cameraOpticMoveSpeed;
	config.pitch -= (ypos - preY) * config.cameraOpticMoveSpeed;
	config.yaw = fmod(config.yaw, 360);
	config.pitch = clamp(config.pitch, -89.f, 89.f);
	preX = xpos;
	preY = ypos;
}

void scroll_callbcak(GLFWwindow* window, double xoffset, double yoffset) {
	for (auto each : config.models) {

		
			each->scale.x += config.scaleSpeed * yoffset;
			each->scale.y += config.scaleSpeed * yoffset;
			each->scale.z += config.scaleSpeed * yoffset;
	
		
	}
}

void draw_line(Model* model, glm::mat4& modelMat, glm::mat4& viewMat, glm::mat4& projectionMat) {
	config.lineShader->use();
	config.lineShader->setMat4("model", modelMat);
	config.lineShader->setMat4("view", viewMat);
	config.lineShader->setMat4("projection", projectionMat);
	glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
	model->draw(config.lineShader->id);
}

void draw_background(glm::mat4 view, glm::mat4 projection) {
	glm::mat4 model(1.0f);
	glm::vec3 tmp = { config.cameraPos.x+1,config.cameraPos.y,config.cameraPos.z};
	
	model = glm::translate(model, tmp);
	//model = glm::scale(model, {10,10,10});
	model = glm::rotate(model, glm::radians(90.0f), glm::vec3(0.0, 0.0, 1.0));
	bgModel->shaderSetMatrixes(model, view, projection);
	glDisable(GL_DEPTH_TEST);
	bgModel->draw();
	glEnable(GL_DEPTH_TEST);
}
bool first = true;
int max_num = 0;
float eighttime;
int updateIndividuals(double currentFrameTime,std::string &outstr)//每调用一次就更新每个个体的速度，所有力的计算在这里进行
{
	int count = 0;
	
	static double lasttime =0;
	
	
	if (currentFrameTime - lasttime >= 0.3||lasttime==0)
	{
		lasttime = currentFrameTime;
		

		for (int i = 0; i < config.N_indiv; i++)//目前是随机给速度
		{

			//保存之前的速度方向
			//individuals[i].getPreDir() = individuals[i].getVel();
			//计算新的方向
			float x, z;
			Map::getcoordinate(N_grid * N_grid - N_grid / 2, x, z);
			if (!individuals[i].has_shadow|| individuals[i].helped_id != -1)continue;
				glm::vec3 fother = calcFOther(individuals[i]);
				glm::vec3 fobstacle = calcFObstacle(individuals[i]);
				glm::vec3 fgroup;
				if ( individuals[i].help_id != -1||!start_simulation)
					fgroup = glm::vec3(0, 0, 0);
				else
					fgroup = calcGroupForce(individuals[i]);
				
				offsetx = (rand() % 20 - 10) / 50.0;
				offsetz = (rand() % 20 - 10) / 50.0;
				glm::vec3 fself = start_simulation? (mymap.get()->directions[getGridId(individuals[i].getTran())]-individuals[i].getVel()):glm::vec3(offsetx,0, offsetz);
				if (start_simulation) 
				{
					if (glm::distance(individuals[i].getTran(), glm::vec3(x, 0, z)) < 6)fself = glm::normalize(glm::vec3(x, 1, z) - individuals[i].getTran());
					//这里还是继续打补丁
					if (is_in_obstacle(individuals[i].getTran() + fself))
					{
						int tmp = getGridId(individuals[i].getTran());
						int adj = tmp;
						Map::getcoordinate(tmp, x, z);
						if (individuals[i].getTran().x > x)adj += 1;
						else adj -= 1;
						if (adj!=tmp)
						{
							if (individuals[i].getTran().z > z)adj += N_grid;
							else adj -= N_grid;
						}
						
						fself = mymap.get()->directions[adj] - individuals[i].getVel();
					}
				}
				
				individuals[i].getVel() += fother.operator*=(0.5)+fobstacle.operator*=(2)+ fgroup.operator*=(0.1)+fself.operator*=(0.5);
				//限制最大速度
				if (individuals[i].getVel().length() > individuals[i].max_vel* (1-individuals[i].DE))
					individuals[i].getVel() = glm::normalize(individuals[i].getVel()).operator*=(individuals[i].max_vel * (1 - individuals[i].DE));

				
				//individuals[i].getVel() *= 1;//速度为0就不会显示，很奇怪的bug,是旋转那边的锅
				/*glm::rotate(glm::mat4(1.0f), 45.0 * ((rand() % 20 - 10) / 20.0), glm::vec3(0,1,0));
				individuals[i].getVel() =*/
				//逃生成功的处理
				count += individuals[i].has_shadow;
				if (is_in_destination(individuals[i].getTran()))
				{
					individuals[i].getTran() = { 100,1,100 };
					
					individuals[i].has_shadow = false;
					if (individuals[i].help_id != -1)
					{
						individuals[individuals[i].help_id].getTran() = { 100,1,100 };

						individuals[individuals[i].help_id].has_shadow = false;
					}
				}
				

		}
		//帮助的处理
		for (int i = 0; i < config.N_indiv; i++)//目前是随机给速度
		{
			if (individuals[i].help_id != -1)
			{
				individuals[individuals[i].help_id].getTran() = individuals[i].getTran() + glm::vec3(0,2,0);
				individuals[individuals[i].help_id].getVel() = individuals[i].getVel();
			}
		}

		if (start_simulation)
		{
			//std::cout << "仿真持续时间：" << currentFrameTime - starttime << "剩余人数" << count << ",被严重挤压的人数：" << hurtcount << std::endl;
			
			if (max_num < hurtcount)max_num = hurtcount;
			hurtcount = 0;
			if (first && count < config.N_indiv * 0.2)
			{ 
				eighttime = currentFrameTime - starttime;
			first = false; }

		}
		if (count == 0&&start_simulation)
		{
			//std::cout << "仿真结束！" << endl;
			//std::cout << "过程最大受伤人数："<<max_num<<",全部人数撤离时间："<< currentFrameTime - starttime <<",80%的人撤离时间:"<<eighttime << endl;
			stringstream out;
			out << "hurted people " << max_num << " ; 100% evacuated : " << currentFrameTime - starttime << " ; 80% evacuated : " << eighttime;
			string tmp;
			while (out>>tmp)
			{
				outstr += tmp+" ";
			}
			
			return 1;
		}

	}
	return 0;
}
float g(float x)
{
	if (x > 0) return x;
	else return 0;
}
glm::vec3 calcFOther(Individual& individual)
{
	//1.find the surrounding grids
	auto individual_ids=mymap->getNearByIndiv(individual);
	glm::vec3 forces = glm::vec3(0,0,0);
	float r_ij,d_ij;
	glm::vec3 n_ij,t_ij;
	int count = 0;
	for (auto i : individual_ids)//计算每个周围个体对当前个体造成的力
	{
		if (i != individual.id&&individuals[i].helped_id==-1) {
			//calc tmp variables
			r_ij = individuals[i].r_body + individual.r_body;
			d_ij = glm::distance(individuals[i].getTran(), individual.getTran())+0.1;//防止除以0，而崩溃
			if (d_ij < 1.3)count++;
			n_ij = (individual.getTran() - individuals[i].getTran()) / d_ij;
			t_ij = glm::vec3(-n_ij.z, 0, n_ij.x);
			//calc force 
			auto f_stayaway = (individual.A * glm::exp(r_ij - d_ij) / individual.B) * n_ij;
			auto f_body = n_ij * g(r_ij - d_ij);
			f_body *= K_fij;
			auto f_sliding = g(r_ij - d_ij) * glm::dot(individuals[i].getVel() - individual.getVel(), t_ij) * t_ij;
			f_sliding *= k_fij;
			//add force 
			forces += f_body + f_sliding + f_stayaway;
		}
	}
	if (count > 2)hurtcount++;

	return forces;
}
glm::vec3 calcFObstacle(Individual& individual)
{
	//1.find the surrounding grids
	auto Obstacles_pos = mymap->getNearByObstacle(individual);
	glm::vec3 forces = glm::vec3(0, 0, 0);
	float r_ij, d_ij;
	glm::vec3 n_ij, t_ij;
	for (auto i : Obstacles_pos)//计算每个周围个体对当前个体造成的力
	{

			//calc tmp variables
			r_ij = individual.r_body;
			float o_x, o_z;
			Map::getcoordinate(i, o_x, o_z);
			d_ij = glm::distance(glm::vec3(o_x,0,o_z), individual.getTran()) + 0.1;//防止除以0，而崩溃
			n_ij = (individual.getTran() - glm::vec3(o_x, 1, o_z)) / d_ij;//注意，物体由于旋转后其y为1
			t_ij = glm::vec3(-n_ij.z, 0, n_ij.x);
			//calc force 
			auto f_stayaway = (individual.A * glm::exp(r_ij - d_ij) / individual.B) * n_ij;
			auto f_body = n_ij * g(r_ij - d_ij);
			f_body *= K_fij;
			auto f_sliding = -g(r_ij - d_ij) * glm::dot(individual.getVel(), t_ij) * t_ij;//为什么公式里面有一个负号
			f_sliding *= k_fij;
			//add force 
			forces += f_body + f_sliding + f_stayaway;

	}


	return forces;
}
glm::vec3 calcGroupForce(Individual& individual)
{
	//1.find the surrounding grids
	auto individual_ids = mymap->getNearByIndiv(individual);
	glm::vec3 forces = glm::vec3(0, 0, 0);
	float x, z;
	Map::getcoordinate(N_grid * N_grid - N_grid / 2,x,z);
	for (auto i : individual_ids)
	{
		if (i != individual.id&& individuals[i].getTexture()== individual.getTexture() && individuals[i].helped_id == -1) {
			float d_ij = glm::distance(individuals[i].getTran(), individual.getTran());

			auto e_ij = glm::normalize(individuals[i].getTran() - individual.getTran()+glm::vec3(rand()%10/100.,0, rand() % 10 / 100.));//防止两个坐标一样
			float d_ip = glm::distance(individual.getTran(), glm::vec3(x, 0, z));
			forces += e_ij.operator*=(K_group * individuals[i].DE * individual.AL * glm::abs(d_ij - d_ip));
			if (d_ij <= 2&& individual.DE< individuals[i].DE &&individuals[i].help_id == -1&& individuals[i].helped_id == -1&& individual.help_id==-1)//施加帮助
			{
				individual.help_id = i;
				individuals[i].helped_id = individual.id;
			}
		}
	}

	return forces;
}
int getRectId(QRect& rect)
{
	int c_x = rect.x() / 10;
	int c_z = rect.y() / 10;
	return c_z * N_grid + c_x;
}
void Map::initObstacle(std::vector<QRect*>& wall_list)//初始化障碍物的位置
{
	for (int i = 0; i < N_grid* N_grid; i++)
	{
		has_obstacle[i] = false;
	}
	for (auto i : wall_list)
	{
		has_obstacle[getRectId(*i)] = true;
		obstacle.push_back(getRectId(*i));
	}
}
void Map::setObstacle(int id)
{
	has_obstacle[id] = true;
}

void Map::getcoordinate(int id, float& x, float& z)
{
	//get grid coordinate
	int c_x = id % N_grid-N_grid/2;
	int c_z = id / N_grid-N_grid / 2;
	//get the center place of the grid
	x = c_x * DIVIDER + 1;
	z = c_z * DIVIDER + 1;
}
//这个函数是为了实现转向用的，可以不搞
float Individual::getAngle(glm::vec3& prevel, glm::vec3& vel)//获取从pre方向到vel方向旋转的角度（角度制）,需要找最小的角度，绕y轴转，符合右手准则的为正
{

	int sign = glm::cross(prevel, vel).y>0?1:-1;
	float tmp = glm::dot(prevel, vel);
	float out= sign * glm::acos(glm::dot(prevel, vel) / glm::sqrt(glm::dot(prevel, prevel) * glm::dot(vel, vel)));
	return out;
}


void Map::flashIndivPalce()//更新每个个体的位置
{
	for (int num=0;num<config.N_indiv;num++)
	{
		auto& i = individuals[num];
		if (!i.has_shadow || i.helped_id != -1)continue;
		//获取个体所在的格子
		int grid_id=glm::floor((i.getTran().x) / DIVIDER) + glm::floor((i.getTran().z) / DIVIDER) * N_grid+N_grid*N_grid/2+ N_grid / 2;
		//清空上一次的状态
		grid_count[grid_id] = 0;
		//更新格子中记录的个体的编号
		grid_indiv[grid_id* MAX_INDIVI_PER_GRID+grid_count[grid_id]] = i.id;
		//更新格子中记录的个体数量
		grid_count[grid_id] += 1;

	}

}
std::vector<int> Map::getNearByIndiv(Individual indivi)
{
	//获取个体所在的格子
	auto t1 = glm::floor((indivi.getTran().x) / DIVIDER);
	auto t2 = (glm::floor((indivi.getTran().z) / DIVIDER) + N_grid / 2) * N_grid;
	int grid_id = glm::floor((indivi.getTran().x) / DIVIDER) + (glm::floor((indivi.getTran().z) / DIVIDER)+ N_grid / 2 )* N_grid + N_grid / 2;
	int c_x = grid_id % N_grid;
	int c_z = grid_id / N_grid;
	//获取该格子周围的格子的编号
	int x_min = c_x - SEARCH_SCOPE<0?0: c_x - SEARCH_SCOPE;
	int z_min = c_z - SEARCH_SCOPE < 0 ? 0 : c_z - SEARCH_SCOPE;
	int x_max = c_x + SEARCH_SCOPE > N_grid-1 ? N_grid - 1 : c_x + SEARCH_SCOPE;
	int z_max = c_z + SEARCH_SCOPE > N_grid - 1 ? N_grid - 1 : c_z + SEARCH_SCOPE;
	std::vector<int> nearbygrid;
	for (int i = x_min; i <= x_max; i++)
	{
		for (int j = z_min; j <= z_max; j++)
		{
			nearbygrid.push_back(j * N_grid + i);
		}
	}
	std::vector<int> nearbyindiv;
	//获取周围格子中的个体的编号
	for (auto grid : nearbygrid)
	{
		for (int i = 0; i < grid_count[grid]; i++)
		{
			nearbyindiv.push_back(grid_indiv[grid * MAX_INDIVI_PER_GRID + i]);
		}
	}
		
	return nearbyindiv;
}
std::vector<int> Map::getNearByObstacle(Individual indivi)
{
	//获取个体所在的格子
	auto t1 = glm::floor((indivi.getTran().x) / DIVIDER);
	auto t2 = (glm::floor((indivi.getTran().z) / DIVIDER) + N_grid / 2) * N_grid;
	int grid_id = glm::floor((indivi.getTran().x) / DIVIDER) + (glm::floor((indivi.getTran().z) / DIVIDER) + N_grid / 2) * N_grid + N_grid / 2;
	int c_x = grid_id % N_grid;
	int c_z = grid_id / N_grid;
	//获取该格子周围的格子的编号
	int x_min = c_x - SEARCH_SCOPE < 0 ? 0 : c_x - SEARCH_SCOPE;
	int z_min = c_z - SEARCH_SCOPE < 0 ? 0 : c_z - SEARCH_SCOPE;
	int x_max = c_x + SEARCH_SCOPE > N_grid - 1 ? N_grid - 1 : c_x + SEARCH_SCOPE;
	int z_max = c_z + SEARCH_SCOPE > N_grid - 1 ? N_grid - 1 : c_z + SEARCH_SCOPE;
	std::vector<int> nearbygrid;
	for (int i = x_min; i <= x_max; i++)
	{
		for (int j = z_min; j <= z_max; j++)
		{
			if(has_obstacle[j * N_grid + i])
			nearbygrid.push_back(j * N_grid + i);
		}
	}
	return nearbygrid;
}

void Map::add_extra_obstacle(std::vector<QRect*>& extra_wall_list)
{
	for (auto i : extra_wall_list)
	{
		has_obstacle[getRectId(*i)] = true;
		extra_obstacle.push_back(getRectId(*i));
	}

}
void Map::removeObstacle(int id)//移除给定位置的障碍物
{
	has_obstacle[id] =false;
}
void Map::remove_extra_obstacle()//关闭额外的墙
{
	for (auto i : extra_obstacle)
		removeObstacle(i);
	extra_obstacle.clear();
}
